{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# Frobenius Norm Factors"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n",
        "\n",
        "These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n",
        "\n",
        "* `FrobeniusPrior<T>`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n",
        "* `FrobeniusFactor<T>`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T_1 - T_2||_F^2$.\n",
        "* `FrobeniusBetweenFactorNL<T>`: Penalizes the Frobenius norm difference between the predicted measurement `T2^{-1} * T1` and the actual measurement `T12_measured`. Error is $||I - T_2^{-1} T_1 \\cdot \\tilde T_{12}||_F^2$. Use this with *any* Matrix Lie group.\n",
        "* `FrobeniusBetweenFactor<T>`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T_1 \\cdot \\tilde T_{12} - T_2||_F^2$. Uses this - easier to optimize factor - if the norm is invariant to multiplying with T2, as is the case for `Rot3`, `Pose3`, etc, but not, say, `Similarity3`. \n",
        "\n",
        "The helper function function `ConvertModel` ensures the noise model used by the various Frobenius factors has the correct dimension. You can either provide:\n",
        "  - an isotropic noise model of the manifold dimension of the type T (e.g., 3 for Rot3)\n",
        "  - any noise model of the full vectorized matrix dimension (N*N, where N is the matrix size, e.g., 9 for Rot3).\n",
        "  -  If you provide an isotropic model of the manifold dimension, ConvertModel will automatically convert it to the correct dimension for the Frobenius factor."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/FrobeniusFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Note: you may need to restart the kernel to use updated packages.\n"
          ]
        }
      ],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import (SL4, FrobeniusBetweenFactorNLSL4,\n",
        "                   FrobeniusBetweenFactorRot3, FrobeniusFactorRot3,\n",
        "                   FrobeniusPriorRot3, Rot3, Values, symbol_shorthand)\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "R = symbol_shorthand.R # Using 'R' for Rot3 keys"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fprior_header_md"
      },
      "source": [
        "## 1. `FrobeniusPrior<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fprior_desc_md"
      },
      "source": [
        "Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "fprior_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "FrobeniusPriorRot3:   keys = { r0 }\n",
            "isotropic dim=9 sigma=0.01\n",
            "FrobeniusPriorRot3 (full 9D noise):   keys = { r0 }\n",
            "isotropic dim=9 sigma=0.01\n",
            "\n",
            "FrobeniusPrior error (vectorized matrix diff): 0.9999916666944463\n"
          ]
        }
      ],
      "source": [
        "target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n",
        "key = R(0)\n",
        "\n",
        "# Option 1: Provide an isotropic noise model of the manifold dimension (3 for Rot3)\n",
        "rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n",
        "# ConvertModel will expand this to 9D for Frobenius factors\n",
        "\n",
        "prior_fro = FrobeniusPriorRot3(key, target_matrix, rot_noise_model)\n",
        "prior_fro.print(\"FrobeniusPriorRot3: \")\n",
        "\n",
        "# Option 2: Provide any noise model of the full vectorized dimension (9 for Rot3)\n",
        "frobenius_noise_model = gtsam.noiseModel.Isotropic.Sigma(9, 0.01)\n",
        "prior_fro_full = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model)\n",
        "prior_fro_full.print(\"FrobeniusPriorRot3 (full 9D noise): \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(key, Rot3.Yaw(0.11))\n",
        "error_prior = prior_fro.error(values)\n",
        "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ffactor_header_md"
      },
      "source": [
        "## 1. `FrobeniusFactor<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ffactor_desc_md"
      },
      "source": [
        "Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "ffactor_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "FrobeniusFactorRot3:   keys = { r0 r1 }\n",
            "isotropic dim=9 sigma=0.02\n",
            "\n",
            "FrobeniusFactor error (vectorized matrix diff): 0.062499869791775416\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "key1 = R(0)\n",
        "key2 = R(1)\n",
        "frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n",
        "factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n",
        "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n",
        "\n",
        "# Evaluate error\n",
        "values.insert(key1, Rot3.Yaw(0.11))\n",
        "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n",
        "error_factor = factor_fro.error(values)\n",
        "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fbetween_header_md"
      },
      "source": [
        "## 3. `FrobeniusBetweenFactor<Rot3>`\n",
        "\n",
        "Acts like `BetweenFactor<Rot3>` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "fbetween_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactorNL<gtsam::Rot3>(r0,r1)\n",
            "  T12:  [\n",
            "\t0.999988, -0.00499998, 0;\n",
            "\t0.00499998, 0.999988, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "isotropic dim=9 sigma=0.005\n",
            "\n",
            "FrobeniusBetweenFactor error: 7.703719777548943e-30\n"
          ]
        }
      ],
      "source": [
        "measured_R12 = Rot3.Yaw(0.005)\n",
        "# Use same noise model dimension (9)\n",
        "frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n",
        "\n",
        "between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n",
        "between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n",
        "\n",
        "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n",
        "error_between = between_fro.error(values)\n",
        "print(f\"\\nFrobeniusBetweenFactor error: {error_between}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 4. `FrobeniusBetweenFactorNL<SL4>`\n",
        "\n",
        "Acts like `BetweenFactor<SL4>` but minimizes $||T_2^{-1} T_1 \\cdot T_{12} - I||_F^2$, for use with groups that do not satisfy the invariance property needed to use `FrobeniusBetweenFactor`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "FrobeniusBetweenFactorNL<SL4>: FrobeniusBetweenFactorNL<gtsam::SL4>(x0,x1)\n",
            "  T12:    1.00505 0.00503769 0.00503769 0.00502517\n",
            "0.00503769    1.00004 0.00502515 0.00501265\n",
            "0.00503769 0.00502515    1.00004 0.00501265\n",
            "0.00502517 0.00501265 0.00501265    0.99505\n",
            "isotropic dim=16 sigma=0.01\n",
            "\n",
            "FrobeniusBetweenFactorNL<SL4> error: 0.003941370929502147\n"
          ]
        }
      ],
      "source": [
        "# Create two SL4 elements (4x4 special linear group matrices)\n",
        "sl4_1 = SL4.Expmap([0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1, 0.11, 0.12, 0.13, 0.14, 0.15])\n",
        "sl4_2 = SL4.Expmap([0.015, 0.025, 0.035, 0.045, 0.055, 0.065, 0.075, 0.085, 0.095, 0.105, 0.115, 0.125, 0.135, 0.145, 0.155])\n",
        "measured_sl4 = SL4.Expmap([0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005])\n",
        "\n",
        "# Create keys for the variables\n",
        "key_sl4_1 = X(0)\n",
        "key_sl4_2 = X(1)\n",
        "\n",
        "# Noise model for 16D (4x4 matrix)\n",
        "sl4_noise_model = gtsam.noiseModel.Isotropic.Sigma(16, 0.01)\n",
        "\n",
        "# Construct the FrobeniusBetweenFactorNL for SL4\n",
        "fbf_sl4 = FrobeniusBetweenFactorNLSL4(key_sl4_1, key_sl4_2, measured_sl4, sl4_noise_model)\n",
        "\n",
        "# Insert values into Values container\n",
        "values_sl4 = Values()\n",
        "values_sl4.insert(key_sl4_1, sl4_1)\n",
        "values_sl4.insert(key_sl4_2, sl4_2)\n",
        "\n",
        "# Print factor and evaluate error\n",
        "fbf_sl4.print(\"\\nFrobeniusBetweenFactorNL<SL4>: \")\n",
        "error_sl4 = fbf_sl4.error(values_sl4)\n",
        "print(f\"\\nFrobeniusBetweenFactorNL<SL4> error: {error_sl4}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}